// C Source File
// Created 12/2/2007; 12:14:38 AM

#include "Search.h"
#include "Terrain.h"
#include "Civilization.h"
#include "Crash.h"
#include "Game.h"

SqQueue opened, closed;
int xFinal, yFinal;
int curCiv;
int maxDist;

// doubles the size of the array
void resize_o()
{
	opened.size *= 2;
	Square *_root = (Square*)Malloc(opened.size * sizeof(Square));
	if(_root == NULL)
	{
		Free(opened.root);
		char str[25];
		sprintf(str, "Search Queue O: %d", opened.size);
		Crash(MEMORY, str);
	}
	
	int i;
	for(i = 0; i < opened.numb; i++)
		_root[i] = opened.root[i];
	
	Free(opened.root);
	opened.root = _root;
}
void resize_c()
{
	closed.size *= 2;
	Square *_root = (Square*)Malloc(closed.size * sizeof(Square));
	if(_root == NULL)
	{
		Free(closed.root);
		char str[25];
		sprintf(str, "Search Queue C: %d", closed.size);
		Crash(MEMORY, str);
	}
	
	int i;
	for(i = 0; i < closed.numb; i++)
		_root[i] = closed.root[i];
	
	Free(closed.root);
	closed.root = _root;
}

// inserts based on position
int insert_o(Square sq)
{
	if(opened.size == opened.numb)
		resize_o();
		
	int pos = sq.x + sq.y*game.mapWidth;
	int low = 0;
	int high = opened.numb - 1;
	
	while(low <= high)
	{
		int mid = (low + high) / 2;
		if(pos < opened.root[mid].x + opened.root[mid].y*game.mapWidth)
			high = mid - 1;
		else
			low = mid + 1;
	}
	
	int z; for(z = opened.numb-1; z >= high+1; z--) opened.root[z+1] = opened.root[z];
	opened.root[high+1] = sq;
	opened.numb++;
	
	return high + 1;
}
int insert_c(Square sq)
{
	if(closed.size == closed.numb)
		resize_c();
		
	int pos = sq.x + sq.y*game.mapWidth;
	int low = 0;
	int high = closed.numb - 1;
	
	while(low <= high)
	{
		int mid = (low + high) / 2;
		if(pos < closed.root[mid].x + closed.root[mid].y*game.mapWidth)
			high = mid - 1;
		else
			low = mid + 1;
	}
	
	int z; for(z = closed.numb-1; z >= high+1; z--) closed.root[z+1] = closed.root[z];
	closed.root[high+1] = sq;
	closed.numb++;
	
	return high + 1;
}

// finds based on position
int find_pos_o(int pos)
{
	int low = 0;
	int high = opened.numb - 1;
	
	while(low <= high)
	{
		int mid = (low + high) / 2;
		if(pos < opened.root[mid].x + opened.root[mid].y*game.mapWidth)
			high = mid - 1;
		else if(pos > opened.root[mid].x + opened.root[mid].y*game.mapWidth)
			low = mid + 1;
		else
			return mid;
	}
	
	return -1;
}
int find_pos_c(int pos)
{
	int low = 0;
	int high = closed.numb - 1;
	
	while(low <= high)
	{
		int mid = (low + high) / 2;
		if(pos < closed.root[mid].x + closed.root[mid].y*game.mapWidth)
			high = mid - 1;
		else if(pos > closed.root[mid].x + closed.root[mid].y*game.mapWidth)
			low = mid + 1;
		else
			return mid;
	}
	
	return -1;
}

// looks through all
int find_min_dis()
{
	int minI = 0;
	int i;
	for(i = 1; i < opened.numb; i++)
		if(opened.root[i].dist_trav + opened.root[i].dist_left < opened.root[minI].dist_trav + opened.root[minI].dist_left)
			minI = i;
	return minI;
}

// removes a square
void remove_pos(int sq)
{
	int i;
	for(i = sq; i < opened.numb; i++)
		opened.root[i] = opened.root[i+1];
	opened.numb--;
}

/*void DebugQueues()
{
	ClrScr();
	DrawStr(0, 0, "Opened", A_NORMAL);
	int i;
	for(i = 0; i < opened.numb; i++)
	{
		char square[40];
		sprintf(square, "[%d] (%d,%d) P:%d D:%d", i, opened.root[i].x, opened.root[i].y, opened.root[i].parent, opened.root[i].dist_trav);
		//sprintf(square, "[%d] [%d] P:%d D:%d", i, opened.root[i].x + opened.root[i].y*game.mapWidth, opened.root[i].parent, opened.root[i].dist_trav);
		DrawStr(5, i*8+10, square, A_NORMAL);
	}
	
	DrawStr(80, 0, "Closed", A_NORMAL);
	for(i = 0; i < closed.numb; i++)
	{
		char square[40];
		sprintf(square, "[%d] (%d,%d) P:%d D:%d", i, closed.root[i].x, closed.root[i].y, closed.root[i].parent, closed.root[i].dist_trav);
		//sprintf(square, "[%d] [%d] P:%d D:%d", i, closed.root[i].x + closed.root[i].y*game.mapWidth, closed.root[i].parent, closed.root[i].dist_trav);
		DrawStr(85, i*8+10, square, A_NORMAL);
	}
	ngetchx();
}*/

// constructor like, takes name, parent, dist_trav, dist_left, x, y
#define SQUARE(S, P, T, L, X, Y) Square S; S.parent = P; S.dist_trav = T; S.dist_left = L; S.x = X; S.y = Y;

// adds a square to a queue
void addToQueue(int x, int y, int parent, int dist_trav)
{
	int z = x+y*game.mapWidth;
	if(dist_trav > maxDist || z == parent) return;
	if(game.worldmap[z] == 1 && GetBit(civilization[curCiv].fog, x + y * game.mapWidth)) return; 
	if(find_pos_c(z) != -1) return;
	int find = find_pos_o(x + y*game.mapWidth);
	
	if(find == -1)
	{
		SQUARE(sq, parent, dist_trav, max(abs(x - xFinal),abs(x - yFinal)), x, y);
		insert_o(sq);
	}
	else
	{
		if(opened.root[find].dist_trav > dist_trav)
		{
			opened.root[find].dist_trav = dist_trav;
			opened.root[find].parent = parent;
		}
	}
}

// finds a path
int getPath(int x1, int y1, int x2, int y2, int *x3, int *y3, int civ)
{
	return _getPath(x1, y1, x2, y2, x3, y3, civ, 15);
}

int _getPath(int x1, int y1, int x2, int y2, int *x3, int *y3, int civ, int _maxDist)
{
#ifdef DEBUG
	static int count = 0;
	char cStr[30];
	Clr(0,0,80,8);
	sprintf(cStr, "getpath: %d dist: %d", count++, _maxDist);
	DrawStr(0, 0, cStr, A_NORMAL);
#endif

	if(x1 == x2 && y1 == y2) { x3[0] = x1; y3[0] = y1; return 0; }
	if(game.worldmap[x2 + y2*game.mapWidth] == 1 && GetBit(civilization[curCiv].fog, x2 + y2 * game.mapWidth)) return -1;
	
	opened.root = (Square*)Malloc(20 * sizeof(Square));
	closed.root = (Square*)Malloc(20 * sizeof(Square));
	opened.size = 20;
	closed.size = 20;
	opened.numb = 0;
	closed.numb = 0;
	
	xFinal = x2;
	yFinal = y2;
	curCiv = civ;
	maxDist = _maxDist * 12;
	
	SQUARE(start, -1, 0, max(abs(x1 - x2),abs(y1 - y2)), x1, y1);
	
	insert_o(start);
	int find = -1;
	
	while((find = find_pos_c(x2 + y2*game.mapWidth)) == -1 && opened.numb != 0)
	{
		int min = find_min_dis();
		int cur = insert_c(opened.root[min]);
		remove_pos(min);
		int parent = closed.root[cur].x + closed.root[cur].y*game.mapWidth;
		
		if(closed.root[cur].y > 0)
			addToQueue(closed.root[cur].x, closed.root[cur].y-1, parent, closed.root[cur].dist_trav + 10);
		if(closed.root[cur].y+1 < game.mapHeight)
			addToQueue(closed.root[cur].x, closed.root[cur].y+1, parent, closed.root[cur].dist_trav + 10);
		
		if(closed.root[cur].x > 0)
		{
			addToQueue(closed.root[cur].x-1, closed.root[cur].y, parent, closed.root[cur].dist_trav + 10);
			if(closed.root[cur].y > 0)
				addToQueue(closed.root[cur].x-1, closed.root[cur].y-1, parent, closed.root[cur].dist_trav + 14);
			if(closed.root[cur].y+1 < game.mapHeight)
				addToQueue(closed.root[cur].x-1, closed.root[cur].y+1, parent, closed.root[cur].dist_trav + 14);
		}
		if(closed.root[cur].x+1 < game.mapWidth)
		{
			addToQueue(closed.root[cur].x+1, closed.root[cur].y, parent, closed.root[cur].dist_trav + 10);
			if(closed.root[cur].y > 0)
				addToQueue(closed.root[cur].x+1, closed.root[cur].y-1, parent, closed.root[cur].dist_trav + 14);
			if(closed.root[cur].y+1 < game.mapHeight)
				addToQueue(closed.root[cur].x+1, closed.root[cur].y+1, parent, closed.root[cur].dist_trav + 14);
		}
	}
	
	int distance = 0;
	if(opened.numb == 0)
	{
		distance = -1;
	}
	else
	{
		while(closed.root[find].parent != -1 && distance < 30)
		{
			x3[distance] = closed.root[find].x;
			y3[distance++] = closed.root[find].y;
			find = find_pos_c(closed.root[find].parent);
		}
		
		x3[distance] = closed.root[find].x;
		y3[distance] = closed.root[find].y;
		
		if(closed.root[find].parent != -1) distance = -1;
	}
	
	Free(opened.root);
	Free(closed.root);
	
	return distance;
}